/**     ----------------------------------------------------------------
*
*   @brief this file contains extra routines needed for the main test program for sgp4.
*   @author david vallado
*
*
*    this file contains extra routines needed for the main test program for sgp4.
*    these routines are derived from the astro libraries.
*
*    companion code for fundamentals of astrodynamics and applications
*                                    2007
*                              by david vallado
*
*       (w) 719-573-2600, email dvallado@agi.com
*
*    current :
*               7 may 08  david vallado
*                           fix sgn
*    changes :
*               2 apr 07  david vallado
*                           fix jday floor and str lengths
*                           updates for constants
*              14 aug 06  david vallado
*                           original baseline
*       ----------------------------------------------------------------      */

#include "sgp4_math.h"

/**
 * @brief Indica el signo de un numero pasado como parametro
 * @param x numero del que queremos conocer su signo
 * @return nos indicara con -1 numero negativo y +1 positivo
 *
 */

 double  sgp4_math::sgn(double x){
     if (x < 0.0){
          return -1.0;
      }else{
          return 1.0;
      }

}  // end sgn

/** -----------------------------------------------------------------------------
*
*   @brief  this procedure finds the magnitude of a vector
*   @param vector vector compuesto por 3 double
*   @return double
*   @author david vallado
*
*                           function mag
*
*  this procedure finds the magnitude of a vector.  the tolerance is set to
*    0.000001, thus the 1.0e-12 for the squared test of underflows.
*
*  author        : david vallado                  719-573-2600    1 mar 2001
*
*  inputs          description                    range / units
*    vec         - vector
*
*  outputs       :
*    vec         - answer stored in fourth component
*
*  locals        :
*    none.
*
*  coupling      :
*    none.
* --------------------------------------------------------------------------- */

 /**
  * @brief Tratando a x como un vector de tres componentes, esta función devuelve el módulo de ese vector.
  * @param x Array que representa un vector. Debe tener reservados tres componentes de tipo double, es decir, x debe ser x[3]
  * @return el módulo del vector x.
  */
 double  sgp4_math::mag(double x[3]){
     return sqrt(x[0]*x[0] + x[1]*x[1] + x[2]*x[2]);
}  // end mag

/** -----------------------------------------------------------------------------
*   @brief  this procedure crosses two vectors.
*   @param vec1 vector compuesto por 3 double
*   @param vec2 vector compuesto por 3 double
*   @param outvec vector compuesto por 3 double, resultado
*
*   @author david vallado
*                           procedure cross
*
*  this procedure crosses two vectors.
*
*  author        : david vallado                  719-573-2600    1 mar 2001
*
*  inputs          description                    range / units
*    vec1        - vector number 1
*    vec2        - vector number 2
*
*  outputs       :
*    outvec      - vector result of a x b
*
*  locals        :
*    none.
*
*  coupling      :
*    mag           magnitude of a vector
 ---------------------------------------------------------------------------- */
 /**
  * @brief Esta función obtiene el producto vectorial de los dos primeros vectores pasados por parámetros y lo devuelve en el tercer vector pasado por parámetro,
  * @param vec1 Array de tres componentes <b>double</b>, que representa el primer vector pasado por parámetro.
  * @param vec2 Array de tres componentes <b>double</b>, que representa el segundo vector pasado por parámetro.
  * @param outvec Array de tres componentes <b>double</b>, que representa el resultado del producto vectorial de vec1 por vec2 (vec1 x vec2). Cualquier valor que tuviera antes de la función, se perderá.
  */
 void    sgp4_math::cross(double vec1[3], double vec2[3], double outvec[3]){
     outvec[0]= vec1[1]*vec2[2] - vec1[2]*vec2[1];
     outvec[1]= vec1[2]*vec2[0] - vec1[0]*vec2[2];
     outvec[2]= vec1[0]*vec2[1] - vec1[1]*vec2[0];
}  // end cross


/** -----------------------------------------------------------------------------
*   @brief  this function finds the dot product of two vectors..
*   @param vec1 vector compuesto por 3 double
*   @param vec2 vector compuesto por 3 double
*   @return double  resultado de aplicar la funcion
*   @author david vallado
*
*                           function dot
*
*  this function finds the dot product of two vectors.
*
*  author        : david vallado                  719-573-2600    1 mar 2001
*
*  inputs          description                    range / units
*    vec1        - vector number 1
*    vec2        - vector number 2
*
*  outputs       :
*    dot         - result
*
*  locals        :
*    none.
*
*  coupling      :
*    none.
*
* --------------------------------------------------------------------------- */

 /**
  * @brief Esta función obtiene el producto escalar de dos vectores pasados por parámetro.
  * @param x Primer array que representa un vector. Tiene que tener reservado, como mínimo, tres componentes de tipo <b>double</b>.
  * @param y Segundo array que representa el otro vector. También tiene que tener reservado, como mínimo, tres componentes de tipo <b>double</b>.
  * @return El producto escalar (x · y), es decir, devuelve exáctamente "x[0]*y[0] + x[1]*y[1] + x[2]*y[2]".
  */
 double  sgp4_math::dot(double x[3], double y[3]){
     return (x[0]*y[0] + x[1]*y[1] + x[2]*y[2]);
}  // end dot

/** -----------------------------------------------------------------------------
*   @brief  this procedure calculates the angle between two vectors.
*   @param vec1 vector compuesto por 3 double
*   @param vec2 vector compuesto por 3 double
*   @return double  angulo de los dos vectores
*   @author david vallado
*
*                           procedure angle
*
*  this procedure calculates the angle between two vectors.  the output is
*    set to 999999.1 to indicate an undefined value.  be sure to check for
*    this at the output phase.
*
*  author        : david vallado                  719-573-2600    1 mar 2001
*
*  inputs          description                    range / units
*    vec1        - vector number 1
*    vec2        - vector number 2
*
*  outputs       :
*    theta       - angle between the two vectors  -pi to pi
*
*  locals        :
*    temp        - temporary real variable
*
*  coupling      :
*    dot           dot product of two vectors
* --------------------------------------------------------------------------- */

 /**
  *@brief Esta función devuelve el ángulo mínimo formado por dos vectores
  *@param vec1 Array de, al menos, tres componentes de tipo <b>double</b>, que representa al primer vector.
  *@param vec2 El otro vector de tipo Array con, al menos, tres componentes de tipo <b>double</b>.
  *@return El ángulo formado por ambos vectores por medio de la función arcocoseno(vec1 · vec2 / (|vec1|*|vec2|)). O, en el caso de que, al menos, el módulo de uno de los vectores sea muy cercano a 0, devolverá 999999.1
  */
 double  sgp4_math::angle(double vec1[3],double vec2[3]){
     double small, undefined, magv1, magv2, temp;
     small     = 0.00000001;
     undefined = 999999.1;

     magv1 = mag(vec1);
     magv2 = mag(vec2);

     if (magv1*magv2 > small*small){
         temp= dot(vec1,vec2) / (magv1*magv2);
         if (fabs( temp ) > 1.0)
             temp= sgn(temp) * 1.0;
         return acos( temp );
     }else
        return undefined;
}  // end angle


/** -----------------------------------------------------------------------------
*
*   @brief  this function evaluates the inverse hyperbolic sine function.
*   @param xval angle value
*   @return arcsinh     - result
*   @author david vallado
*
*                           function asinh
*
*  this function evaluates the inverse hyperbolic sine function.
*
*  author        : david vallado                  719-573-2600    1 mar 2001
*
*  inputs          description                    range / units
*    xval        - angle value                                  any real
*
*  outputs       :
*    arcsinh     - result                                       any real
*
*  locals        :
*    none.
*
*  coupling      :
*    none.
*
* --------------------------------------------------------------------------- */

 /**
  * @brief Esta función devuelve el arcoseno hiperbólico del valor introducido.
  * @param xval variable para calcular su arcoseno hiperbólico
  * @return El arcoseno hiperbólico de xval.
  */
double  sgp4_math::asinh(double xval){
     return log( xval + sqrt( xval*xval + 1.0 ) );
}  // end asinh


/** -----------------------------------------------------------------------------
*   @brief   this function solves keplers equation when the true anomaly is known.
*   @param ecc         - eccentricity                   0.0  to
*   @param nu          - true anomaly                   -2pi to 2pi rad
*   @param e0  output        - eccentric anomaly              0.0  to 2pi rad       153.02 �
*   @param m   output        - mean anomaly                   0.0  to 2pi rad       151.7425 �*
*   @author david vallado
*
*                           function newtonnu
*
*  this function solves keplers equation when the true anomaly is known.
*    the mean and eccentric, parabolic, or hyperbolic anomaly is also found.
*    the parabolic limit at 168� is arbitrary. the hyperbolic anomaly is also
*    limited. the hyperbolic sine is used because it's not double valued.
*
*  author        : david vallado                  719-573-2600   27 may 2002
*
*  revisions
*    vallado     - fix small                                     24 sep 2002
*
*  inputs          description                    range / units
*    ecc         - eccentricity                   0.0  to
*    nu          - true anomaly                   -2pi to 2pi rad
*
*  outputs       :
*    e0          - eccentric anomaly              0.0  to 2pi rad       153.02 �
*    m           - mean anomaly                   0.0  to 2pi rad       151.7425 �
*
*  locals        :
*    e1          - eccentric anomaly, next value  rad
*    sine        - sine of e
*    cose        - cosine of e
*    ktr         - index
*
*  coupling      :
*    asinh       - arc hyperbolic sine
*
*  references    :
*    vallado       2007, 85, alg 5
* --------------------------------------------------------------------------- */

/**
 * @brief Esta función resuelve la ecuación de Kepler, cuando la verdadera anomalía es conocida.
 * @param ecc Valor de la excentricidad
 * @param nu Valor de la verdadera anomalía. Este valor debe estar comprendido entre -2pi y 2pi radianes.
 * @param e0 Parámetro para obtener la anomalía excéntrica. Perderá el valor que tuviera antes de llamar a ésta función.
 * @param m Parámetro para obtener la anomalía media. Perderá el valor que tuviera antes de llamar a ésta función.
 */
 void sgp4_math::newtonnu(double ecc, double nu,double& e0, double& m){
       double small, sine, cose;

     // ---------------------  implementation   ---------------------
     e0= 999999.9;
     m = 999999.9;
     small = 0.00000001;

     // --------------------------- circular ------------------------
     if ( fabs( ecc ) < small  ){
         m = nu;
         e0= nu;
     }else
         // ---------------------- elliptical -----------------------
         if ( ecc < 1.0-small  ){
             sine= ( sqrt( 1.0 -ecc*ecc ) * sin(nu) ) / ( 1.0 +ecc*cos(nu) );
             cose= ( ecc + cos(nu) ) / ( 1.0  + ecc*cos(nu) );
             e0  = atan2( sine,cose );
             m   = e0 - ecc*sin(e0);
           }else
             // -------------------- hyperbolic  --------------------
             if ( ecc > 1.0 + small  ){
                 if ((ecc > 1.0 ) && (fabs(nu)+0.00001 < pi-acos(1.0 /ecc))){
                     sine= ( sqrt( ecc*ecc-1.0  ) * sin(nu) ) / ( 1.0  + ecc*cos(nu) );
                     e0  = log( sine + sqrt( sine*sine + 1.0 ) );
                     m   = ecc*sinh(e0) - e0;
                   }
              }else{
                 // ----------------- parabolic ---------------------
                 if ( fabs(nu) < 168.0*pi/180.0  ){
                     e0= tan( nu*0.5  );
                     m = e0 + (e0*e0*e0)/3.0;
                 }
              }

     if ( ecc < 1.0  ){
         m = fmod( m,2.0 *pi );
         if ( m < 0.0  )
             m = m + 2.0 *pi;
         e0 = fmod( e0,2.0 *pi );
       }
   }  // end newtonnu


/** -----------------------------------------------------------------------------
*   @brief   this function finds the classical orbital elements given the geocentric
*   equatorial position and velocity vectors.
*    @param  r           - ijk position vector            km
*    @param  v           - ijk velocity vector            km / s
*    @param  mu          - gravitational parameter        km3 / s2
*    @param p           output - semilatus rectum               km
*    @param a           output - semimajor axis                 km
*    @param ecc         output - eccentricity
*    @param incl        output - inclination                    0.0  to pi rad
*    @param omega       output - longitude of ascending node    0.0  to 2pi rad
*    @param argp        output - argument of perigee            0.0  to 2pi rad
*    @param nu          output - true anomaly                   0.0  to 2pi rad
*    @param m           output - mean anomaly                   0.0  to 2pi rad
*    @param arglat      output - argument of latitude      (ci) 0.0  to 2pi rad
*    @param truelon     output - true longitude            (ce) 0.0  to 2pi rad
*    @param lonper      output - longitude of periapsis    (ee) 0.0  to 2pi rad
*    @author david vallado
*
*                           function rv2coe
*
*  this function finds the classical orbital elements given the geocentric
*    equatorial position and velocity vectors.
*
*  author        : david vallado                  719-573-2600   21 jun 2002
*
*  revisions
*    vallado     - fix special cases                              5 sep 2002
*    vallado     - delete extra check in inclination code        16 oct 2002
*    vallado     - add constant file use                         29 jun 2003
*    vallado     - add mu                                         2 apr 2007
*
*  inputs          description                    range / units
*    r           - ijk position vector            km
*    v           - ijk velocity vector            km / s
*    mu          - gravitational parameter        km3 / s2
*
*  outputs       :
*    p           - semilatus rectum               km
*    a           - semimajor axis                 km
*    ecc         - eccentricity
*    incl        - inclination                    0.0  to pi rad
*    omega       - longitude of ascending node    0.0  to 2pi rad
*    argp        - argument of perigee            0.0  to 2pi rad
*    nu          - true anomaly                   0.0  to 2pi rad
*    m           - mean anomaly                   0.0  to 2pi rad
*    arglat      - argument of latitude      (ci) 0.0  to 2pi rad
*    truelon     - true longitude            (ce) 0.0  to 2pi rad
*    lonper      - longitude of periapsis    (ee) 0.0  to 2pi rad
*
*  locals        :
*    hbar        - angular momentum h vector      km2 / s
*    ebar        - eccentricity     e vector
*    nbar        - line of nodes    n vector
*    c1          - v**2 - u/r
*    rdotv       - r dot v
*    hk          - hk unit vector
*    sme         - specfic mechanical energy      km2 / s2
*    i           - index
*    e           - eccentric, parabolic,
*                  hyperbolic anomaly             rad
*    temp        - temporary variable
*    typeorbit   - type of orbit                  ee, ei, ce, ci
*
*  coupling      :
*    mag         - magnitude of a vector
*    cross       - cross product of two vectors
*    angle       - find the angle between two vectors
*    newtonnu    - find the mean anomaly
*
*  references    :
*    vallado       2007, 126, alg 9, ex 2-5
* --------------------------------------------------------------------------- */

 /**
  * @brief Esta función busca los clásicos elementos orbitales, dada la posición geocéntrica ecuatorial y los vectores de velocidad.
  * @param r vector de posición
  * @param v vector de velocidad
  * @param mu parámetro gravitacional
  * @param p semi-latus rectum. Perderá el valor que tuviera antes de llamar a ésta función.
  * @param a semimajor axis. Perderá el valor que tuviera antes de llamar a ésta función.
  * @param ecc excentricidad. Perderá el valor que tuviera antes de llamar a ésta función.
  * @param incl inclinación. Perderá el valor que tuviera antes de llamar a ésta función.
  * @param omega longitud del nodo ascendente. Perderá el valor que tuviera antes de llamar a ésta función.
  * @param argp argumento del perigeo. Perderá el valor que tuviera antes de llamar a ésta función.
  * @param nu anomalía verdadera. Perderá el valor que tuviera antes de llamar a ésta función.
  * @param m anomalía media. Perderá el valor que tuviera antes de llamar a ésta función.
  * @param arglat argumento de latitud. Perderá el valor que tuviera antes de llamar a ésta función.
  * @param truelon longitud cerdadera. Perderá el valor que tuviera antes de llamar a ésta función.
  * @param lonper longitud de periapsis. Perderá el valor que tuviera antes de llamar a ésta función.
  */
void sgp4_math::rv2coe(
       double r[3], double v[3], double mu,
       double& p, double& a, double& ecc, double& incl, double& omega, double& argp,
       double& nu, double& m, double& arglat, double& truelon, double& lonper
     ){
       double undefined, small, hbar[3], nbar[3], magr, magv, magn, ebar[3], sme,
              rdotv, infinite, temp, c1, hk, twopi, magh, halfpi, e;

       int i;
       char typeorbit[3];

     twopi  = 2.0 * pi;
     halfpi = 0.5 * pi;
     small  = 0.00000001;
     undefined = 999999.1;
     infinite  = 999999.9;

     // -------------------------  implementation   -----------------
     magr = mag( r );
     magv = mag( v );

     // ------------------  find h n and e vectors   ----------------
     cross( r,v, hbar );
     magh = mag( hbar );
     if ( magh > small ){
         nbar[0]= -hbar[1];
         nbar[1]=  hbar[0];
         nbar[2]=   0.0;
         magn = mag( nbar );
         c1 = magv*magv - mu /magr;
         rdotv = dot( r,v );
         for (i= 0; i <= 2; i++)
             ebar[i]= (c1*r[i] - rdotv*v[i])/mu;
         ecc = mag( ebar );

         // ------------  find a e and semi-latus rectum   ----------
         sme= ( magv*magv*0.5  ) - ( mu /magr );
         if ( fabs( sme ) > small )
             a= -mu  / (2.0 *sme);
           else
             a= infinite;
         p = magh*magh/mu;

         // -----------------  find inclination   -------------------
         hk= hbar[2]/magh;
         incl= acos( hk );

         // --------  determine type of orbit for later use  --------
         // ------ elliptical, parabolic, hyperbolic inclined -------
         strcpy(typeorbit,"ei");
         if ( ecc < small ){
             // ----------------  circular equatorial ---------------
             if  ((incl<small) | (fabs(incl-pi)<small))
                 strcpy(typeorbit,"ce");
               else
                 // --------------  circular inclined ---------------
                 strcpy(typeorbit,"ci");
           }else{
             // - elliptical, parabolic, hyperbolic equatorial --
             if  ((incl<small) | (fabs(incl-pi)<small))
                 strcpy(typeorbit,"ee");
           }

         // ----------  find longitude of ascending node ------------
         if ( magn > small ){
             temp= nbar[0] / magn;
             if ( fabs(temp) > 1.0  )
                 temp= sgn(temp);
             omega= acos( temp );
             if ( nbar[1] < 0.0  )
                 omega= twopi - omega;
           }else
             omega= undefined;

         // ---------------- find argument of perigee ---------------
         if ( strcmp(typeorbit,"ei") == 0 ){
             argp = angle( nbar,ebar);
             if ( ebar[2] < 0.0  )
                 argp= twopi - argp;
           }else
             argp= undefined;

         // ------------  find true anomaly at epoch    -------------
         if ( typeorbit[0] == 'e' ){
             nu =  angle( ebar,r);
             if ( rdotv < 0.0  )
                 nu= twopi - nu;
           }else
             nu= undefined;

         // ----  find argument of latitude - circular inclined -----
         if ( strcmp(typeorbit,"ci") == 0 ){
             arglat = angle( nbar,r );
             if ( r[2] < 0.0  )
                 arglat= twopi - arglat;
             m = arglat;
           }
           else
             arglat= undefined;

         // -- find longitude of perigee - elliptical equatorial ----
         if  (( ecc>small ) && (strcmp(typeorbit,"ee") == 0))
           {
             temp= ebar[0]/ecc;
             if ( fabs(temp) > 1.0  )
                 temp= sgn(temp);
             lonper= acos( temp );
             if ( ebar[1] < 0.0  )
                 lonper= twopi - lonper;
             if ( incl > halfpi )
                 lonper= twopi - lonper;
           }
           else
             lonper= undefined;

         // -------- find true longitude - circular equatorial ------
         if  (( magr>small ) && ( strcmp(typeorbit,"ce") == 0 ))
           {
             temp= r[0]/magr;
             if ( fabs(temp) > 1.0  )
                 temp= sgn(temp);
             truelon= acos( temp );
             if ( r[1] < 0.0  )
                 truelon= twopi - truelon;
             if ( incl > halfpi )
                 truelon= twopi - truelon;
             m = truelon;
           }
           else
             truelon= undefined;

         // ------------ find mean anomaly for all orbits -----------
         if ( typeorbit[0] == 'e' )
             newtonnu(ecc,nu,  e, m);
     }
      else
     {
        p    = undefined;
        a    = undefined;
        ecc  = undefined;
        incl = undefined;
        omega= undefined;
        argp = undefined;
        nu   = undefined;
        m    = undefined;
        arglat = undefined;
        truelon= undefined;
        lonper = undefined;
     }
   }  // end rv2coe





